#include <msp430x26x.h>
#include "UART_HANDLER.h"
#include "PortDef.h"
#include <stdio.h>

/******************************************************
 * VERSION NUMBER - manually update this as substantive changes are made
 ******************************************************/
const char *VER_NUM = "1.0 C";   // this is the version number
/******************************************************/

/******************************************************
 * INTERRUPT DECLARES - declare ISRs and link their handlers
 ******************************************************/
__interrupt void usart1_rx(void);
USCIAB1RX_ISR(usart1_rx)
/******************************************************/

/******************************************************
Variable Declarations, initializations 
******************************************************/
struct UART_RX_BUF S_UART_BUFFER; //declare the buffer object to be used
unsigned char UART_ON,MENU_ON,CHANNELSET_ON;
/*****************************************************/

/******************************************************
 * InitSys - initialize and configure system after reset
 ******************************************************/
void InitSys(void)
{
// *********************** CLOCKS ****************************************
 	//XT1 has a 32.768kHz crystal attached
 	//XT2 has a 16MHz crystal attached
 	int i;
 	WDTCTL = WDTPW + WDTHOLD;   // Stop WDT
 	_disable_interrupts();		// DINT
 	_bic_SR_register(OSCOFF);	// Turn on oscillators
 	BCSCTL1 = 0x00;				// XT2 ON, LFXT1 = low freq, ACLK div = 1
	BCSCTL3 = (XT2S_2+LFXT1S_0+XCAP_3); //XT2 uses 16MHz crystal, XT1 in low freq op, XT1 uses 12.5pf cap, 
	do
		{
			IFG1 &= ~(OFIFG);				// Clear OFIFG
			for(i = 0; i<255;i++) _nop();	// Delay
		}
	while(IFG1&OFIFG);			// Loop while OFIFG bit in IFG1 = 1
	BCSCTL2 = (SELM_2+DIVM_0+SELS+DIVS_1);		// MCLK=XT2, div=1, SMCLK=XT2, div = 2
	//so, at this point the ACLK=32768Hz, MCLK=16MHz, SMCLK=8MHz, DCO is off

// *********************** PORTS *****************************************

	//setup outputs (low by default)
  	P1OUT = 0;
	P2OUT = (0+SPIEnblGyroZ+SPIEnblAccel+SPIEnblPres);	// SPI enables are active low, so deactivate by setting high
	P3OUT = (0+SPIEnblMag);
	P4OUT = 0;
	P5OUT = 0;
	P6OUT = 0;

//  I/O configured as outputs by default to eliminate floating inputs.
  	P1DIR = 0xFF;
  	P2DIR = (0xFF & ~MagDatRdy & ~PresDatRdy); 	//MagDatRdy, PresDatRdy are inputs
  	P3DIR = (0xFF & ~SPIMiSo);	//MiSo is an input
  	P4DIR = 0xFF;   // All P4.x outputs
  	P5DIR = 0xFF;   // All P5.x outputs
  	P6DIR = 0xFF;	// All P6.x outputs
	
// Set up pin special functions.
    P1SEL = (Serv1PWMOut+Serv2PWMOut);	//PWM outputs from Timer
    P3SEL = (UART_TxToGPS+UART_RxFromGPS);	//connect to UARTs
	P4SEL = (Mot1PWMOut+Mot2PWMOut+Serv1PWMIn+Serv2PWMIn+Serv3PWMIn+Serv4PWMIn);	//connect to timer module
	P6SEL = (Son1In+Son2In+Son3In+Son4In+GyroX+GyroY+GyroXYRef);	//connect to ADC
	
// *********************** UART ******************************************
	//configure UCA1 (for GPS or debug terminal)
  	UCA1CTL0 = 0;						// 8N1 UART config
  	UCA1CTL1 |= (UCSSEL_2+UCSWRST);     // SMCLK source and hold UART in reset
  	UCA1BR0 = 13;                    	// see tables in user guide
  	UCA1BR1 = 0;                    	// 
  	UCA1MCTL = UCOS16;      			// oversampling mode
  	UCA1CTL1 &= ~UCSWRST;             	// **Initialize USCI state machine**
  	UC1IE |= UCA1RXIE;                  // Enable USCI_A1 RX interrupt
    

  // Initialize the RX buffer structure.  
	for(i=0;i<60;i++) S_UART_BUFFER.Buffer[i]=0;
	S_UART_BUFFER.Buf_Strt = &S_UART_BUFFER.Buffer[0]; 	//get the address of the buffer head
	S_UART_BUFFER.Buf_End = &S_UART_BUFFER.Buffer[0];  	//get the address of the buffer tail
	S_UART_BUFFER.Buffer_Length = 0;					//initialize the buffer length to 0
	
	UART_ON = 1;									// variable to indicate that the UART is open to programming
	MENU_ON = 0;
	
	tx_byte_to_slave(0x0D);
	tx_byte_to_slave(0x0A);
	tx_slave_string("IMUboard restart");
	tx_byte_to_slave(0x0D);
	tx_byte_to_slave(0x0A);

	
// ************************ Timer A  ******
 
// ************************ Timer B ******

						
	_enable_interrupts();	//this is the last thing that the Init Routine should do, since we don't want ISR activity before everything is initialized  		
}
/************************************************************************/

/*********************************************************
main - the main scheduling and code loop
*********************************************************/
void main(void)
{
	InitSys();
	
	while(1) //main polling loop 
	{
		if(S_UART_BUFFER.Buffer_Length > 0) S_parseRXCmd();	// if receive buffer contains data, decide what to do w/ it
	}
}
/*********************************************************/

/*********************************************************
ISR: usart0_rx - this code gets executed whet USART0 receives a byte
*********************************************************/
__interrupt void usart1_rx(void)
{
	tx_byte_to_slave(UCA1RXBUF); //echo the byte out 
	S_UART_BUFFER.Buffer[S_UART_BUFFER.Buffer_Length] = UCA1RXBUF;  // Copy Data
	if(S_UART_BUFFER.Buffer_Length < 60) S_UART_BUFFER.Buffer_Length++; // increment buffer length
	else S_UART_BUFFER.Buffer_Length = 0;  // reset buffer if we would have written past the end    
}
/********************************************************/

